#ifndef __UDSF_PROTOCOL_PCRST_C__
#define __UDSF_PROTOCOL_PCRST_C__

#include "app.h"
/*
*   notice:
*   UDSF 同UDF编程：不可调用cstd-lib,只可调用"sdk_ifs_udk_cfg.h"、"sys_api.h"中列出的API函数。
*   UDSF文件内函数均需使用static修饰符，且UDF中以#include "udsf_xxx.c"形式引用
*/

#if defined(FMCW_RADAR_4D)
static struct pc_rst_4d* udsf_pcrst_init(pc_rst_4d_t * p_pcrst)
#else
static struct pc_rst* udsf_pcrst_init(pc_rst_t * p_pcrst)
#endif
{
	p_pcrst->targetMax  = PCRST_MAX;
	p_pcrst->target     = p_pcrst->target_buff;
	p_pcrst->targetNum  = 0;
	#if defined(FMCW_RADAR_4D)
	return (struct pc_rst_4d*)p_pcrst;
	#else
	return (struct pc_rst*)p_pcrst;
	#endif
}
#if defined(FMCW_RADAR_4D)
static void udsf_pcrst_copy_with_init_rfilter( struct pc_rst_4d *pcrst_src,pc_rst_4d_t *pcrst_dst,uint16_t range_th_s,uint16_t range_th_e)
#else
static void udsf_pcrst_copy_with_init_rfilter( struct pc_rst *pcrst_src,pc_rst_t *pcrst_dst,uint16_t range_th_s,uint16_t range_th_e)
#endif
{
	int cnt = 0;

	pcrst_dst->targetMax  = PCRST_MAX;
	pcrst_dst->target     = pcrst_dst->target_buff;
	pcrst_dst->targetNum = 0;
	
	for( int i=0;i<pcrst_src->targetNum;i++){
		if(pcrst_src->target[i].d1Idx < range_th_s || pcrst_src->target[i].d1Idx > range_th_e){
			continue;
		}
		pcrst_dst->target[cnt] = pcrst_src->target[i];
		cnt++;
	}
	pcrst_dst->targetNum = cnt;
}

#if defined(FMCW_RADAR_4D)
static void udsf_epc_init(epc_4d_t *p_epc,
uint8_t rangeMax,uint8_t chirpMax,uint16_t range_res_mm,uint16_t vel_res_mm_s,uint8_t res_decimals,uint16_t range_offset_mm,uint16_t addon_rsv)
#else
static void udsf_epc_init(epc_t *p_epc,
uint8_t rangeMax,uint8_t chirpMax,uint16_t range_res_mm,uint16_t vel_res_mm_s,uint8_t res_decimals,uint16_t range_offset_mm,uint16_t addon_rsv)
#endif
{
	p_epc->target_cnt            = 0;
	p_epc->targetMax             = RADAR_EPC_NUM_MAX;
	p_epc->p_target              = p_epc->target_buff;
	p_epc->rangeMax              = rangeMax;
	p_epc->chirpMax              = chirpMax;
	p_epc->angleMax              = NUM_ANGLE;
	p_epc->range_res_mm          = range_res_mm;
	p_epc->vel_res_mm_s          = vel_res_mm_s;
	p_epc->addon.res_decimals    = res_decimals;
	p_epc->addon.range_offset_mm = range_offset_mm;
	p_epc->addon.rsv             = addon_rsv;
}

#if !defined(FMCW_RADAR_4D)
static void udsf_cluster_format2_append_to_epc(DBSCANOUT *DBscanClu ,struct epc *p_epc,uint32_t cluster_filter_vth,uint8_t ex_r,uint8_t ex_v,uint8_t ex_a,uint8_t info1,uint8_t info2){

	clusterRpt * p_cluster = DBscanClu->cluster;
	struct epc_target  * p_epc_target = p_epc->p_target;
	
	for( int i=0;i<DBscanClu->numCluster;i++ ){
		if( p_epc->target_cnt < p_epc->targetMax ){
			if( p_cluster[i].vIdxMean < cluster_filter_vth ){
				continue;
			}
			p_epc_target[p_epc->target_cnt].rIdx      = p_cluster[i].rIdxMean*ex_r;      
			p_epc_target[p_epc->target_cnt].vIdx      = p_cluster[i].vIdxMean*ex_v;      
			p_epc_target[p_epc->target_cnt].sinPhiIdx = p_cluster[i].sinPhiIdxMean*ex_a;

			p_epc_target[p_epc->target_cnt].pow       = p_cluster[i].abs;
			if( (info1>>6)&0x2 ){
				p_epc_target[p_epc->target_cnt].presence_flag = 1;
			}else{
				p_epc_target[p_epc->target_cnt].presence_flag = 0;
			}
			p_epc_target[p_epc->target_cnt].info1     = info1;
			p_epc_target[p_epc->target_cnt].info2     = info2;

			p_epc->target_cnt++;
    }else{
			break;
		}
	}
}

static void udsf_cluster_append_to_epc(DBSCANOUT *DBscanClu ,struct epc *p_epc,uint32_t cluster_filter_vth,uint8_t info1,uint8_t info2){
	
	clusterRpt * p_cluster = DBscanClu->cluster;
	struct epc_target  * p_epc_target = p_epc->p_target;
	
	info1 &= (~(7<<3));
	info1 |= (1<<3);  // 聚类格式1
	int cnt = p_epc->target_cnt;

	for( int i=0;i<DBscanClu->numCluster;i++ ){
		if( cnt < p_epc->targetMax ){
				if( p_cluster[i].vIdxMean < cluster_filter_vth ){
					continue;
				}
				p_epc_target[cnt].rIdx      = p_cluster[i].rIdxMean;      
				p_epc_target[cnt].vIdx      = p_cluster[i].vIdxMean;      
				p_epc_target[cnt].sinPhiIdx = p_cluster[i].sinPhiIdxMean;

				p_epc_target[cnt].pow       = p_cluster[i].abs;
				if( (info1>>6) &0x2){
					p_epc_target[cnt].presence_flag = 1;
				}else{
					p_epc_target[cnt].presence_flag = 0;
				}
				//if( presence_flag ){
					p_epc_target[cnt].info1     = info1;
				//}else{
				//	p_epc_target[p_epc->target_cnt].info1     = info1;
				//}
				p_epc_target[cnt].info2     = info2;
			
				cnt++;
		}
		else
		{
				break;
		}
	}
	
	p_epc->target_cnt = cnt;
}
#endif

#if defined(FMCW_RADAR_4D)
static void udsf_pcrst_append_to_epc(struct pc_rst_4d * frameRst,struct epc_4d *p_epc,uint8_t pt_class_id,uint8_t pt_style)
#else
static void udsf_pcrst_append_to_epc(struct pc_rst * frameRst,struct epc *p_epc,uint8_t pt_class_id,uint8_t pt_style)
#endif
{
	#if defined(FMCW_RADAR_4D)
	struct epc_target_4d  * p_epc_target   = p_epc->p_target;
	STRUCT_TARGETPOINT_4D * p_pcrst_target = frameRst->target;
	#else
	STRUCT_TARGETPOINT * p_pcrst_target = frameRst->target;
	struct epc_target  * p_epc_target   = p_epc->p_target;
	#endif

	for( int i=0;i<frameRst->targetNum;i++ ){
		
		if( p_epc->target_cnt < p_epc->targetMax ){
			
			p_epc_target[p_epc->target_cnt].rIdx = p_pcrst_target[i].d1Idx;
			
			if (p_pcrst_target[i].d2Idx*2 >= p_epc->chirpMax){
				p_epc_target[p_epc->target_cnt].vIdx = p_pcrst_target[i].d2Idx - p_epc->chirpMax;
			}else{
				p_epc_target[p_epc->target_cnt].vIdx = p_pcrst_target[i].d2Idx;
			}
			
			if (p_pcrst_target[i].d3Idx*2 >= p_epc->angleMax){
				p_epc_target[p_epc->target_cnt].sinPhiIdx = p_pcrst_target[i].d3Idx - p_epc->angleMax;
			}else{
				p_epc_target[p_epc->target_cnt].sinPhiIdx = p_pcrst_target[i].d3Idx;
			}

			#if defined(FMCW_RADAR_4D)
			if (p_pcrst_target[i].d4Idx*2 >= p_epc->angleMax){
				p_epc_target[p_epc->target_cnt].VsinPhiIdx = p_pcrst_target[i].d4Idx - p_epc->angleMax;
			}else{
				p_epc_target[p_epc->target_cnt].VsinPhiIdx = p_pcrst_target[i].d4Idx;
			}
			#endif

			p_epc_target[p_epc->target_cnt].pow = p_pcrst_target[i].powABS;

			if( pt_class_id & 0x2 ){
				p_epc_target[p_epc->target_cnt].presence_flag = 1;
			}else{
				p_epc_target[p_epc->target_cnt].presence_flag = 0;
			}
			
			p_epc_target[p_epc->target_cnt].info1     = (pt_style|(0x0<<3)) | (pt_class_id<<6); // 风格、格式、标识
			
			p_epc_target[p_epc->target_cnt].info2     = 0;
			
			p_epc->target_cnt++;
		}else{
			break;
		}
	}
}


#if defined(FMCW_RADAR_4D)
static void udsf_protov12_frame_start(uint8_t* buff,int *p_offset,uint16_t delta,struct epc_4d *p_epc,uint8_t bcd_version,uint8_t RSV)
#else
static void udsf_protov12_frame_start(uint8_t* buff,int *p_offset,uint16_t delta,struct epc*p_epc,uint8_t bcd_version,uint8_t RSV)
#endif
{
	int index = *p_offset;
	
	buff[index++] = 0x55;
	buff[index++] = 0xAA;
	
	/* len */
	buff[index++] = 0x0; 
	buff[index++] = 0x0;
	buff[index++] = 0x0;
	buff[index++] = 0x0;
	
	buff[index++] = delta;
	buff[index++] = delta>>8;
	buff[index++] = 0x01;
	buff[index++] = 0x01;
	buff[index++] = 0x00;
	buff[index++] = 0x00;

	buff[index++] = 0xAB;
	buff[index++] = 0xCD;
	buff[index++] = 0x12;
	buff[index++] = 0;

	buff[index++] = p_epc->rangeMax;
	buff[index++] = p_epc->chirpMax;
	buff[index++] = p_epc->range_res_mm;
	buff[index++] = p_epc->range_res_mm>>8;
	buff[index++] = p_epc->vel_res_mm_s;
	buff[index++] = p_epc->vel_res_mm_s>>8;
	
	buff[index++] = ((uint8_t*)&p_epc->addon)[0];
	buff[index++] = ((uint8_t*)&p_epc->addon)[1];
	buff[index++] = ((uint8_t*)&p_epc->addon)[2];
	buff[index++] = ((uint8_t*)&p_epc->addon)[3];

	/*target cnt*/
	buff[index++] = 0x0;
	buff[index++] = 0x0;
	
	*p_offset = index;
}

static void udsf_protov12_frame_end(uint8_t* buff,int *p_offset,uint16_t target_cnt){
	int len = *p_offset - 6;
	
	buff[2] = len;
	buff[3] = len>>8;
	buff[4] = len>>16;
	buff[5] = len>>24;
	
	buff[26] = target_cnt; 
	buff[27] = target_cnt>>8;
}

#if defined(FMCW_RADAR_4D)
static void udsf_protov12_add_epc_data(uint8_t *buff,int *p_offset,struct epc_4d *p_epc,uint16_t* p_target_cnt)
#else
static void udsf_protov12_add_epc_data(uint8_t *buff,int *p_offset,struct epc *p_epc,uint16_t* p_target_cnt)
#endif
{
	int index = *p_offset;
	#if defined(FMCW_RADAR_4D)
	struct epc_target_4d  * p_epc_target = p_epc->p_target;
	#else
	struct epc_target  * p_epc_target = p_epc->p_target;
	#endif

	for( int i = 0;i< p_epc->target_cnt;i++ ){
		
		buff[index++] = p_epc_target[i].info1;
		buff[index++] = p_epc_target[i].info2;
		//CSTD_PRINTF("info1 %02x\n",p_epc_target[i].info1);
		int data_type = (p_epc_target[i].info1>>3)&0x7;
		if( (data_type == 0x1) || (data_type == 0x2) ){
			udsf_memcpy((uint8_t*)&p_epc_target[i].rIdx,&buff[index],4);
			index += 4;
			udsf_memcpy((uint8_t*)&p_epc_target[i].vIdx,&buff[index],4);
			index += 4;
			udsf_memcpy((uint8_t*)&p_epc_target[i].sinPhiIdx,&buff[index],4);
			index += 4;
			udsf_memcpy((uint8_t*)&p_epc_target[i].pow,&buff[index],4);
			index += 4;
		}
		#if defined(FMCW_RADAR_4D)
		else if( data_type == 0x3){
			uint16_t u16_data = p_epc_target[i].rIdx;
			udsf_memcpy((uint8_t*)&u16_data,&buff[index],2);
			index += 2;
			
			int8_t s8_data = p_epc_target[i].vIdx;
			udsf_memcpy((uint8_t*)&s8_data,&buff[index],1);
			index += 1;
			
			s8_data = p_epc_target[i].sinPhiIdx;
			udsf_memcpy((uint8_t*)&s8_data,&buff[index],1);
			index += 1;

			s8_data = p_epc_target[i].VsinPhiIdx;
			udsf_memcpy((uint8_t*)&s8_data,&buff[index],1);
			index += 1;
			
			udsf_memcpy((uint8_t*)&p_epc_target[i].pow,&buff[index],4);
			index += 4;
		}
		else if( (data_type == 0x4) || (data_type == 0x5) ){
			udsf_memcpy((uint8_t*)&p_epc_target[i].rIdx,&buff[index],4);
			index += 4;
			udsf_memcpy((uint8_t*)&p_epc_target[i].vIdx,&buff[index],4);
			index += 4;
			udsf_memcpy((uint8_t*)&p_epc_target[i].sinPhiIdx,&buff[index],4);
			index += 4;
			udsf_memcpy((uint8_t*)&p_epc_target[i].VsinPhiIdx,&buff[index],4);
			index += 4;
			udsf_memcpy((uint8_t*)&p_epc_target[i].pow,&buff[index],4);
			index += 4;
		}
		#endif
		else{
			uint16_t u16_data = p_epc_target[i].rIdx;
			udsf_memcpy((uint8_t*)&u16_data,&buff[index],2);
			index += 2;
			
			int8_t s8_data = p_epc_target[i].vIdx;
			udsf_memcpy((uint8_t*)&s8_data,&buff[index],1);
			index += 1;
			
			s8_data = p_epc_target[i].sinPhiIdx;
			udsf_memcpy((uint8_t*)&s8_data,&buff[index],1);
			index += 1;
			
			udsf_memcpy((uint8_t*)&p_epc_target[i].pow,&buff[index],4);
			index += 4;
		}
	}

	if( p_target_cnt ){
		*p_target_cnt += p_epc->target_cnt;
	}
	*p_offset = index;
}

static void udsf_protov12_add_app_data( uint8_t *buff,int *p_offset,struct app_protocol *p_app ){
	
	int index = *p_offset;

	buff[index] = p_app->app_id;
	index += 1;
	
	buff[index] = p_app->app_len;
	index += 1;
	
	//CSTD_MEMCPY(&buff[index],p_app->app_data,p_app->app_len);
	udsf_memcpy(p_app->app_data,&buff[index],p_app->app_len);
	index += p_app->app_len;

	*p_offset = index;
}

#endif
